#include "CSerialPort.h"
#include "plane.h"
#include "global.h"

using namespace std;
typedef pcl::PointXYZ PointT;

extern CSerialPort myserialport;

void findplane_run()
{
	sor_plane.setInputCloud(cloud_filtered);
	sor_plane.setLeafSize(0.012f, 0.012f, 0.012f);
	sor_plane.filter(*cloud_filtered_plane);

	ne_plane.setSearchMethod(tree_plane);
	ne_plane.setInputCloud(cloud_filtered_plane);
	ne_plane.setKSearch(20);
	ne_plane.compute(*cloud_normals_plane);

	seg_plane.setOptimizeCoefficients(true);
	seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	seg_plane.setNormalDistanceWeight(0.1);
	seg_plane.setMethodType(pcl::SAC_RANSAC);
	seg_plane.setMaxIterations(100);
	seg_plane.setDistanceThreshold(0.03);
	seg_plane.setInputCloud(cloud_filtered_plane);
	seg_plane.setInputNormals(cloud_normals_plane);
	seg_plane.segment(*inliers_plane, *coefficients_plane);
	
	float a, b, c, d;
	short angle, distance;
	if (coefficients_plane->values.size() > 0)
	{
		a = coefficients_plane->values[0];
		b = coefficients_plane->values[1];
		c = coefficients_plane->values[2];
		d = coefficients_plane->values[3];
		angle = atan2(c, b) * 180 / 3.14 - 90;
		if(angle<-45)angle+=180;
		distance = (abs(d) / sqrt(a*a + b*b + c*c))*100;
		if(c!=0)cout << "角度: " << angle << endl;
		cout << "距离" << distance;
		angle=-angle;;
		send_data[0]=1;
		send_data[1] = angle;
		send_data[2] = (angle >> 8);
		send_data[3] = distance;
		send_data[4] = (distance >> 8);
		send_data[5] = 0;
		send_data[6] = 0;
	}
	else{
		send_data[0]=angle=distance=0;
	}
	send_data[1] = angle;
	send_data[2] = (angle >> 8);
	send_data[3] = distance;
	send_data[4] = (distance >> 8);
	send_data[5] = 0;
	send_data[6] = 0;
	send_data[7]=0x5A;

	//Extract the planar inliers from the input cloud
	extract_plane.setInputCloud(cloud_filtered_plane);
	extract_plane.setIndices(inliers_plane);
	extract_plane.setNegative(false);

	pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
	extract_plane.filter(*cloud_viewer);
	std::cerr << "    Done: " << tt.toc() << " ms\n\n";

	// if(cloud_plane->size()>0)viewer.showCloud(cloud_plane);
}
